home *** CD-ROM | disk | FTP | other *** search
/ AmigActive 26 / AACD 26.iso / AACD / Graphics / sKulpt / skulpt-src / D3d-Math.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  2000-07-18  |  12.2 KB  |  352 lines
  1. //-----------------------------------------------------------------------------
  2. // File: D3DMath.cpp
  3. //
  4. // Desc: Shortcut macros and functions for using DX objects
  5. //
  6. // Copyright (c) 1997-1999 Microsoft Corporation. All rights reserved
  7. //-----------------------------------------------------------------------------
  8. #define D3D_OVERLOADS
  9. #define STRICT
  10. #include <math.h>
  11. #include <stdio.h>
  12. #include "D3DMath.h"
  13.  
  14. //-----------------------------------------------------------------------------
  15. // Name: D3DMath_MatrixMultiply()
  16. // Desc: Does the matrix operation: [Q] = [A] * [B]. Note that the order of
  17. //       this operation was changed from the previous version of the DXSDK.
  18. //-----------------------------------------------------------------------------
  19. VOID D3DMath_MatrixMultiply( D3DMATRIX& q, D3DMATRIX& a, D3DMATRIX& b )
  20. {
  21.     FLOAT* pA = (FLOAT*)&a;
  22.     FLOAT* pB = (FLOAT*)&b;
  23.     FLOAT  pM[16];
  24.  
  25.     ZeroMemory( pM, sizeof(D3DMATRIX) );
  26.  
  27.     for( WORD i=0; i<4; i++ ) 
  28.         for( WORD j=0; j<4; j++ ) 
  29.             for( WORD k=0; k<4; k++ ) 
  30.                 pM[4*i+j] +=  pA[4*i+k] * pB[4*k+j];
  31.  
  32.     memcpy( &q, pM, sizeof(D3DMATRIX) );
  33. }
  34.  
  35.  
  36.  
  37.  
  38. //-----------------------------------------------------------------------------
  39. // Name: D3DMath_MatrixInvert()
  40. // Desc: Does the matrix operation: [Q] = inv[A]. Note: this function only
  41. //       works for matrices with [0 0 0 1] for the 4th column.
  42. //-----------------------------------------------------------------------------
  43. HRESULT D3DMath_MatrixInvert( D3DMATRIX& q, D3DMATRIX& a )
  44. {
  45.     if( fabs(a._m._44 - 1.0f) > .001f)
  46.         return E_INVALIDARG;
  47.     if( fabs(a._m._14) > .001f || fabs(a._m._24) > .001f || fabs(a._m._34) > .001f )
  48.         return E_INVALIDARG;
  49.  
  50.     FLOAT fDetInv = 1.0f / ( a._m._11 * ( a._m._22 * a._m._33 - a._m._23 * a._m._32 ) -
  51.                              a._m._12 * ( a._m._21 * a._m._33 - a._m._23 * a._m._31 ) +
  52.                              a._m._13 * ( a._m._21 * a._m._32 - a._m._22 * a._m._31 ) );
  53.  
  54.     q._m._11 =  fDetInv * ( a._m._22 * a._m._33 - a._m._23 * a._m._32 );
  55.     q._m._12 = -fDetInv * ( a._m._12 * a._m._33 - a._m._13 * a._m._32 );
  56.     q._m._13 =  fDetInv * ( a._m._12 * a._m._23 - a._m._13 * a._m._22 );
  57.     q._m._14 = 0.0f;
  58.  
  59.     q._m._21 = -fDetInv * ( a._m._21 * a._m._33 - a._m._23 * a._m._31 );
  60.     q._m._22 =  fDetInv * ( a._m._11 * a._m._33 - a._m._13 * a._m._31 );
  61.     q._m._23 = -fDetInv * ( a._m._11 * a._m._23 - a._m._13 * a._m._21 );
  62.     q._m._24 = 0.0f;
  63.  
  64.     q._m._31 =  fDetInv * ( a._m._21 * a._m._32 - a._m._22 * a._m._31 );
  65.     q._m._32 = -fDetInv * ( a._m._11 * a._m._32 - a._m._12 * a._m._31 );
  66.     q._m._33 =  fDetInv * ( a._m._11 * a._m._22 - a._m._12 * a._m._21 );
  67.     q._m._34 = 0.0f;
  68.  
  69.     q._m._41 = -( a._m._41 * q._m._11 + a._m._42 * q._m._21 + a._m._43 * q._m._31 );
  70.     q._m._42 = -( a._m._41 * q._m._12 + a._m._42 * q._m._22 + a._m._43 * q._m._32 );
  71.     q._m._43 = -( a._m._41 * q._m._13 + a._m._42 * q._m._23 + a._m._43 * q._m._33 );
  72.     q._m._44 = 1.0f;
  73.  
  74.     return S_OK;
  75. }
  76.  
  77. //-----------------------------------------------------------------------------
  78. //-----------------------------------------------------------------------------
  79. HRESULT D3DMath_MatrixTranspose( D3DMATRIX& q, D3DMATRIX& a )
  80. {
  81.     q._m._11 = a._m._11;
  82.     q._m._12 = a._m._21;
  83.     q._m._13 = a._m._31;
  84.     q._m._14 = a._m._41;
  85.  
  86.     q._m._21 = a._m._12;
  87.     q._m._22 = a._m._22;
  88.     q._m._23 = a._m._32;
  89.     q._m._24 = a._m._42;
  90.  
  91.     q._m._31 = a._m._13;
  92.     q._m._32 = a._m._23;
  93.     q._m._33 = a._m._33;
  94.     q._m._34 = a._m._43;
  95.  
  96.     q._m._41 = a._m._14;
  97.     q._m._42 = a._m._24;
  98.     q._m._43 = a._m._34;
  99.     q._m._44 = a._m._44;
  100.  
  101.     return S_OK;
  102. }
  103.  
  104.  
  105.  
  106.  
  107. //-----------------------------------------------------------------------------
  108. // Name: D3DMath_VectorMatrixMultiply()
  109. // Desc: Multiplies a vector by a matrix
  110. //-----------------------------------------------------------------------------
  111. HRESULT D3DMath_VectorMatrixMultiply( D3DVECTOR& vDest, D3DVECTOR& vSrc,
  112.                                       D3DMATRIX& mat)
  113. {
  114.     FLOAT x = vSrc.x*mat._m._11 + vSrc.y*mat._m._21 + vSrc.z* mat._m._31 + mat._m._41;
  115.     FLOAT y = vSrc.x*mat._m._12 + vSrc.y*mat._m._22 + vSrc.z* mat._m._32 + mat._m._42;
  116.     FLOAT z = vSrc.x*mat._m._13 + vSrc.y*mat._m._23 + vSrc.z* mat._m._33 + mat._m._43;
  117.     FLOAT w = vSrc.x*mat._m._14 + vSrc.y*mat._m._24 + vSrc.z* mat._m._34 + mat._m._44;
  118.     
  119.     if( fabs( w ) < g_EPSILON )
  120.         return E_INVALIDARG;
  121.  
  122.     vDest.x = x/w;
  123.     vDest.y = y/w;
  124.     vDest.z = z/w;
  125.  
  126.     return S_OK;
  127. }
  128.  
  129.  
  130.  
  131.  
  132. //-----------------------------------------------------------------------------
  133. // Name: D3DMath_VertexMatrixMultiply()
  134. // Desc: Multiplies a vertex by a matrix
  135. //-----------------------------------------------------------------------------
  136. HRESULT D3DMath_VertexMatrixMultiply( D3DVERTEX& vDest, D3DVERTEX& vSrc,
  137.                                       D3DMATRIX& mat )
  138. {
  139.     HRESULT    hr;
  140.     D3DVECTOR* pSrcVec  = (D3DVECTOR*)&vSrc.x;
  141.     D3DVECTOR* pDestVec = (D3DVECTOR*)&vDest.x;
  142.  
  143.     if( SUCCEEDED( hr = D3DMath_VectorMatrixMultiply( *pDestVec, *pSrcVec,
  144.                                                       mat ) ) )
  145.     {
  146.         pSrcVec  = (D3DVECTOR*)&vSrc.nx;
  147.         pDestVec = (D3DVECTOR*)&vDest.nx;
  148.         hr = D3DMath_VectorMatrixMultiply( *pDestVec, *pSrcVec, mat );
  149.     }
  150.     return hr;
  151. }
  152.  
  153.  
  154.  
  155.  
  156. //-----------------------------------------------------------------------------
  157. // Name: D3DMath_QuaternionFromRotation()
  158. // Desc: Converts a normalized axis and angle to a unit quaternion.
  159. //-----------------------------------------------------------------------------
  160. VOID D3DMath_QuaternionFromRotation( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
  161.                                      D3DVECTOR& v, FLOAT fTheta )
  162. {
  163.     x = sinf( fTheta/2.0f ) * v.x;
  164.     y = sinf( fTheta/2.0f ) * v.y;
  165.     z = sinf( fTheta/2.0f ) * v.z;
  166.     w = cosf( fTheta/2.0f );
  167. }
  168.  
  169.  
  170.  
  171.  
  172. //-----------------------------------------------------------------------------
  173. // Name: D3DMath_RotationFromQuaternion()
  174. // Desc: Converts a normalized axis and angle to a unit quaternion.
  175. //-----------------------------------------------------------------------------
  176. VOID D3DMath_RotationFromQuaternion( D3DVECTOR& v, FLOAT& fTheta,
  177.                                      FLOAT x, FLOAT y, FLOAT z, FLOAT w )
  178.                                       
  179. {
  180.     fTheta = acosf(w) * 2.0f;
  181.     v.x    = x / sinf( fTheta/2.0f );
  182.     v.y    = y / sinf( fTheta/2.0f );
  183.     v.z    = z / sinf( fTheta/2.0f );
  184. }
  185.  
  186.  
  187.  
  188.  
  189. //-----------------------------------------------------------------------------
  190. // Name: D3DMath_QuaternionFromAngles()
  191. // Desc: Converts euler angles to a unit quaternion.
  192. //-----------------------------------------------------------------------------
  193. VOID D3DMath_QuaternionFromAngles( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
  194.                                    FLOAT fYaw, FLOAT fPitch, FLOAT fRoll )
  195.                                         
  196. {
  197.     FLOAT fSinYaw   = sinf( fYaw/2.0f );
  198.     FLOAT fSinPitch = sinf( fPitch/2.0f );
  199.     FLOAT fSinRoll  = sinf( fRoll/2.0f );
  200.     FLOAT fCosYaw   = cosf( fYaw/2.0f );
  201.     FLOAT fCosPitch = cosf( fPitch/2.0f );
  202.     FLOAT fCosRoll  = cosf( fRoll/2.0f );
  203.  
  204.     x = fSinRoll * fCosPitch * fCosYaw - fCosRoll * fSinPitch * fSinYaw;
  205.     y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
  206.     z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
  207.     w = fCosRoll * fCosPitch * fCosYaw + fSinRoll * fSinPitch * fSinYaw;
  208. }
  209.  
  210.  
  211.  
  212.  
  213. //-----------------------------------------------------------------------------
  214. // Name: D3DMath_MatrixFromQuaternion()
  215. // Desc: Converts a unit quaternion into a rotation matrix.
  216. //-----------------------------------------------------------------------------
  217. VOID D3DMath_MatrixFromQuaternion( D3DMATRIX& mat, FLOAT x, FLOAT y, FLOAT z,
  218.                                    FLOAT w )
  219. {
  220.     FLOAT xx = x*x; FLOAT yy = y*y; FLOAT zz = z*z;
  221.     FLOAT xy = x*y; FLOAT xz = x*z; FLOAT yz = y*z;
  222.     FLOAT wx = w*x; FLOAT wy = w*y; FLOAT wz = w*z;
  223.     
  224.     mat._m._11 = 1 - 2 * ( yy + zz );
  225.     mat._m._12 =     2 * ( xy - wz );
  226.     mat._m._13 =     2 * ( xz + wy );
  227.  
  228.     mat._m._21 =     2 * ( xy + wz );
  229.     mat._m._22 = 1 - 2 * ( xx + zz );
  230.     mat._m._23 =     2 * ( yz - wx );
  231.  
  232.     mat._m._31 =     2 * ( xz - wy );
  233.     mat._m._32 =     2 * ( yz + wx );
  234.     mat._m._33 = 1 - 2 * ( xx + yy );
  235.  
  236.     mat._m._14 = mat._m._24 = mat._m._34 = 0.0f;
  237.     mat._m._41 = mat._m._42 = mat._m._43 = 0.0f;
  238.     mat._m._44 = 1.0f;
  239. }
  240.  
  241.  
  242.  
  243.  
  244. //-----------------------------------------------------------------------------
  245. // Name: D3DMath_QuaternionFromMatrix()
  246. // Desc: Converts a rotation matrix into a unit quaternion.
  247. //-----------------------------------------------------------------------------
  248. VOID D3DMath_QuaternionFromMatrix( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
  249.                                    D3DMATRIX& mat )
  250. {
  251.     if( mat._m._11 + mat._m._22 + mat._m._33 > 0.0f )
  252.     {
  253.         FLOAT s = sqrtf( mat._m._11 + mat._m._22 + mat._m._33 + mat._m._44 );
  254.  
  255.         x = (mat._m._23-mat._m._32) / (2*s);
  256.         y = (mat._m._31-mat._m._13) / (2*s);
  257.         z = (mat._m._12-mat._m._21) / (2*s);
  258.         w = 0.5f * s;
  259.     }
  260.     else
  261.     {
  262.  
  263.  
  264.     }
  265.     FLOAT xx = x*x; FLOAT yy = y*y; FLOAT zz = z*z;
  266.     FLOAT xy = x*y; FLOAT xz = x*z; FLOAT yz = y*z;
  267.     FLOAT wx = w*x; FLOAT wy = w*y; FLOAT wz = w*z;
  268.     
  269.     mat._m._11 = 1 - 2 * ( yy + zz );
  270.     mat._m._12 =     2 * ( xy - wz );
  271.     mat._m._13 =     2 * ( xz + wy );
  272.  
  273.     mat._m._21 =     2 * ( xy + wz );
  274.     mat._m._22 = 1 - 2 * ( xx + zz );
  275.     mat._m._23 =     2 * ( yz - wx );
  276.  
  277.     mat._m._31 =     2 * ( xz - wy );
  278.     mat._m._32 =     2 * ( yz + wx );
  279.     mat._m._33 = 1 - 2 * ( xx + yy );
  280.  
  281.     mat._m._14 = mat._m._24 = mat._m._34 = 0.0f;
  282.     mat._m._41 = mat._m._42 = mat._m._43 = 0.0f;
  283.     mat._m._44 = 1.0f;
  284. }
  285.  
  286.  
  287.  
  288.  
  289. //-----------------------------------------------------------------------------
  290. // Name: D3DMath_QuaternionMultiply()
  291. // Desc: Mulitples two quaternions together as in {Q} = {A} * {B}.
  292. //-----------------------------------------------------------------------------
  293. VOID D3DMath_QuaternionMultiply( FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw,
  294.                                   FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw,
  295.                                   FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw )
  296. {
  297.     FLOAT Dx =  Ax*Bw + Ay*Bz - Az*By + Aw*Bx;
  298.     FLOAT Dy = -Ax*Bz + Ay*Bw + Az*Bx + Aw*By;
  299.     FLOAT Dz =  Ax*By - Ay*Bx + Az*Bw + Aw*Bz;
  300.     FLOAT Dw = -Ax*Bx - Ay*By - Az*Bz + Aw*Bw;
  301.  
  302.     Qx = Dx; Qy = Dy; Qz = Dz; Qw = Dw;
  303. }
  304.  
  305.  
  306.  
  307.  
  308. //-----------------------------------------------------------------------------
  309. // Name: D3DMath_SlerpQuaternions()
  310. // Desc: Compute a quaternion which is the spherical linear interpolation
  311. //       between two other quaternions by dvFraction.
  312. //-----------------------------------------------------------------------------
  313. VOID D3DMath_QuaternionSlerp( FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw,
  314.                               FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw,
  315.                               FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw,
  316.                               FLOAT fAlpha )
  317. {
  318.     // Compute dot product (equal to cosine of the angle between quaternions)
  319.     FLOAT fCosTheta = Ax*Bx + Ay*By + Az*Bz + Aw*Bw;
  320.  
  321.     // Check angle to see if quaternions are in opposite hemispheres
  322.     if( fCosTheta < 0.0f ) 
  323.     {
  324.         // If so, flip one of the quaterions
  325.         fCosTheta = -fCosTheta;
  326.         Bx = -Bx; By = -By; Bz = -Bz; Bw = -Bw;
  327.     }
  328.  
  329.     // Set factors to do linear interpolation, as a special case where the
  330.     // quaternions are close together.
  331.     FLOAT fBeta = 1.0f - fAlpha;
  332.     
  333.     // If the quaternions aren't close, proceed with spherical interpolation
  334.     if( 1.0f - fCosTheta > 0.001f ) 
  335.     {   
  336.         FLOAT fTheta = acosf( fCosTheta );
  337.         
  338.         fBeta  = sinf( fTheta*fBeta ) / sinf( fTheta);
  339.         fAlpha = sinf( fTheta*fAlpha ) / sinf( fTheta);
  340.     }
  341.  
  342.     // Do the interpolation
  343.     Qx = fBeta*Ax + fAlpha*Bx;
  344.     Qy = fBeta*Ay + fAlpha*By;
  345.     Qz = fBeta*Az + fAlpha*Bz;
  346.     Qw = fBeta*Aw + fAlpha*Bw;
  347. }
  348.  
  349.  
  350.  
  351.  
  352.